import math
import threading
import rospy
from your_package.msg import TurntableState

class TurntableController:
    def __init__(self, workpiece_map, publish_rate=10):
        # 工件位置配置（示例：{'A':0, 'B':60,...}）
        self.workpiece_map = workpiece_map  
        self.angle_list = sorted(workpiece_map.values())
        
        # 状态维护
        self.current_angle = 0.0
        self.last_published_angle = None
        self.state_lock = threading.Lock()
        self.current_state = TurntableState()
        
        # ROS发布器
        self.state_pub = rospy.Publisher('/turntable/state', TurntableState, queue_size=10)
        self.publish_timer = rospy.Timer(rospy.Duration(1.0/publish_rate), self.publish_state)

    def _update_workpiece_state(self, angle):
        """更新工件位置状态"""
        with self.state_lock:
            # 计算最近的前后工件
            front_angle = angle % 360
            rear_angle = (angle + 180) % 360
            
            # 查找最近工件
            front_wp, front_diff = self._find_nearest_workpiece(front_angle)
            rear_wp, rear_diff = self._find_nearest_workpiece(rear_angle)
            
            # 判断是否对准
            is_aligned = abs(front_diff) < 1.0 and abs(rear_diff) < 1.0
            
            # 生成新状态
            new_state = TurntableState()
            new_state.front_workpiece = front_wp if abs(front_diff)<1.0 else "N/A"
            new_state.rear_workpiece = rear_wp if abs(rear_diff)<1.0 else "N/A"
            new_state.is_aligned = is_aligned
            
            # 检查是否需要更新
            if self._state_changed(new_state) or abs(angle - self.last_published_angle) > 1.0:
                self.current_state = new_state
                self.last_published_angle = angle
                return True
            return False

    def _find_nearest_workpiece(self, target_angle):
        """查找距离最近的工件"""
        min_diff = 360.0
        nearest_wp = "N/A"
        
        for wp, angle in self.workpiece_map.items():
            diff = abs((target_angle - angle) % 360)
            diff = min(diff, 360 - diff)
            if diff < min_diff:
                min_diff = diff
                nearest_wp = wp
        
        actual_diff = (target_angle - self.workpiece_map[nearest_wp]) % 360
        if actual_diff > 180:
            actual_diff -= 360
        return nearest_wp, actual_diff

    def _state_changed(self, new_state):
        """判断状态是否发生变化"""
        return (new_state.front_workpiece != self.current_state.front_workpiece or
                new_state.rear_workpiece != self.current_state.rear_workpiece or
                new_state.is_aligned != self.current_state.is_aligned)

    def publish_state(self, event=None):
        """定时发布状态"""
        with self.state_lock:
            if self.current_state:
                self.state_pub.publish(self.current_state)

    def update_angle(self, new_angle):
        """外部调用更新当前角度"""
        if self._update_workpiece_state(new_angle):
            self.publish_state()  # 立即发布重要状态变化

    # 其他运动控制方法保持不变...